#include "drv_SG90.h"
#include "drv_systick.h"
uint8_t volatile flag = 0;
void drv_sg90_init(void)
{
	GPIO_InitTypeDef GPIO_InitStruct;
	TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStruct;
	TIM_OCInitTypeDef TIM_OCInitStruct;

	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//开GPIOA时钟
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);//开TIM14时钟
	
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;//复用模式
	GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;//推挽
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11;
	GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;//使能上拉
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;//速率
	GPIO_Init(GPIOA, &GPIO_InitStruct);

	GPIO_PinAFConfig(GPIOA, GPIO_PinSource8, GPIO_AF_TIM1);//把PA8复用为TIM14
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_TIM1);//把PA9复用为TIM1
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_TIM1);//把PA10复用为TIM14
	GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_TIM1);//把PA11复用为TIM14

	TIM_TimeBaseInitStruct.TIM_ClockDivision = TIM_CKD_DIV1;//不分频
	TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;//向上计数
	TIM_TimeBaseInitStruct.TIM_Period = 20000-1;//自动重装载值
	TIM_TimeBaseInitStruct.TIM_Prescaler = 168-1;//预分频
	TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStruct);
	TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);//定时器的更新中断

	TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;//定时器输出模式为PWM1
	TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Reset;//空闲状态为低电平
	TIM_OCInitStruct.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
	TIM_OCInitStruct.TIM_OCPolarity= TIM_OCPolarity_High;//输出比较通道的有效极性为低电平
	TIM_OCInitStruct.TIM_OCNPolarity = TIM_OCNPolarity_High;
	TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;//使能定时器输出比较通道的输出
	TIM_OCInitStruct.TIM_OutputNState = TIM_OutputNState_Disable;
	TIM_OCInitStruct.TIM_Pulse = 500;//PWM的比较值
	
	TIM_OC1Init(TIM1, &TIM_OCInitStruct);
	TIM_OC2Init(TIM1, &TIM_OCInitStruct);
	TIM_OC3Init(TIM1, &TIM_OCInitStruct);
	TIM_OC4Init(TIM1, &TIM_OCInitStruct);

	TIM_ARRPreloadConfig(TIM1, ENABLE);//使能TIM1的自动重装值预装载
	TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);//TIM1通道1的输出比较预装载
	TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
	TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
	TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
	TIM_CtrlPWMOutputs(TIM1, ENABLE);

	TIM_Cmd(TIM1, ENABLE);
	SG90_SetAngle1(90);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	SG90_SetAngle4(90);
}
void SG90_SetAngle1(uint16_t angle)
{
	uint16_t pulse_us;
	pulse_us = 500+500*(angle/45);
	TIM_SetCompare1(TIM1,pulse_us);//定时器1的通道1的比较值
}
void SG90_SetAngle2(uint16_t angle)
{
	uint16_t pulse_us;
	pulse_us = 500+500*(angle/45);
	TIM_SetCompare2(TIM1,pulse_us);
}
void SG90_SetAngle3(uint16_t angle)
{
	uint16_t pulse_us;
	pulse_us = 500+500*((180-angle)/45);
	TIM_SetCompare3(TIM1,pulse_us);
}
void SG90_SetAngle4(uint16_t angle)
{
	uint16_t pulse_us;
	pulse_us = 500+500*((180-angle)/45);
	TIM_SetCompare4(TIM1,pulse_us);
}
void SG90_Go(void)
{
	while(!flag)
	{
		SG90_SetAngle2(45);
		SG90_SetAngle3(45);
		drv_systick_ms(300);
		SG90_SetAngle1(135);
		SG90_SetAngle4(135);
		drv_systick_ms(300);
		
		SG90_SetAngle2(90);
		SG90_SetAngle3(90);
		drv_systick_ms(300);
		SG90_SetAngle1(90);
		SG90_SetAngle4(90);
		drv_systick_ms(300);
	}
}
void SG90_Back(void)
{
	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	drv_systick_ms(300);
	
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);

	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	drv_systick_ms(300);
	
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);

	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	drv_systick_ms(300);
	
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);

	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	drv_systick_ms(300);
	
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);
}
void SG90_Left(void)
{
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);

	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);

	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);

	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);
	
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);

	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);

	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	SG90_SetAngle2(135);
	SG90_SetAngle3(135);
	drv_systick_ms(300);

	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(300);
}
void SG90_Right(void)
{
	SG90_SetAngle2(45);
	SG90_SetAngle3(45);
	SG90_SetAngle1(135);
	SG90_SetAngle4(135);
	drv_systick_ms(300);

	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);

	SG90_SetAngle2(45);
	SG90_SetAngle3(45);
	SG90_SetAngle1(135);
	SG90_SetAngle4(135);
	drv_systick_ms(300);

	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);
	
	SG90_SetAngle2(45);
	SG90_SetAngle3(45);
	SG90_SetAngle1(135);
	SG90_SetAngle4(135);
	drv_systick_ms(300);

	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);

	SG90_SetAngle2(45);
	SG90_SetAngle3(45);
	SG90_SetAngle1(135);
	SG90_SetAngle4(135);
	drv_systick_ms(300);

	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	drv_systick_ms(300);
}
void SG90_Liedown(void)
{
	SG90_SetAngle1(45);
	SG90_SetAngle4(45);
	SG90_SetAngle2(45);
	SG90_SetAngle3(45);
	drv_systick_ms(500);
	
}
void SG90_Sitdown(void)
{
	SG90_SetAngle1(135);
	SG90_SetAngle4(135);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
}
void SG90_Stop(void)
{
	SG90_SetAngle1(90);
	SG90_SetAngle4(90);
	SG90_SetAngle2(90);
	SG90_SetAngle3(90);
	drv_systick_ms(500);
}


